function Weather_UDP_Client(block)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%
	setup(block);

function setup(block)
	input_period  = 1000; %[ms]
	output_period = 1; 	  %[ms]

	block.NumDialogPrms = 3;

	%block.SetPreCompInpPortInfoToDynamic;
  block.SetPreCompOutPortInfoToDynamic;

	block.NumInputPorts = 1;
	block.NumOutputPorts = 2;

  block.InputPort(1).Complexity   = 'Real';
  block.InputPort(1).DataTypeId   = 6;
  block.InputPort(1).SamplingMode = 'Sample';
  block.InputPort(1).Dimensions   = 5;
  block.InputPort(1).SampleTime   = [input_period/1000 0];

  block.OutputPort(1).Complexity   = 'Real';
  block.OutputPort(1).DataTypeId   = 6;
  block.OutputPort(1).SamplingMode = 'Sample';
  block.OutputPort(1).Dimensions   = 3;
  block.OutputPort(1).SampleTime   = [output_period/1000 0];

  block.OutputPort(2).Complexity   = 'Real';
  block.OutputPort(2).DataTypeId   = 3;
  block.OutputPort(2).SamplingMode = 'Sample';
  block.OutputPort(2).Dimensions   = 1;
  block.OutputPort(2).SampleTime   = [output_period/1000 0];

	block.RegBlockMethod('PostPropagationSetup',    @DoPostPropSetup);
	block.RegBlockMethod('Start',                   @Start);  
	block.RegBlockMethod('Outputs',                 @Outputs);
	block.RegBlockMethod('SetInputPortSampleTime',  @PortInputs);
	block.RegBlockMethod('SetOutputPortSampleTime', @PortOutputs);
	block.RegBlockMethod('Terminate',               @Terminate);

function DoPostPropSetup(block)
	block.NumDworks = 2;

	block.Dwork(1).Name            = 'socket';
	block.Dwork(1).Dimensions      = 1;
	block.Dwork(1).DatatypeID      = 0;      % double
	block.Dwork(1).Complexity      = 'Real'; % real
	block.Dwork(1).UsedAsDiscState = true;

	block.Dwork(2).Name            = 'state';
	block.Dwork(2).Dimensions      = 1;
	block.Dwork(2).DatatypeID      = 0;      % double
	block.Dwork(2).Complexity      = 'Real'; % real
	block.Dwork(2).UsedAsDiscState = true;

function Start(block)
	port = block.DialogPrm(2).data;

	udp=pnet('udpsocket',port);
	pnet(udp,'setreadtimeout',1/100000);
	pnet(udp,'setwritetimeout',1/100000);
	block.Dwork(1).Data = udp;
	block.Dwork(2).Data = 0;

function Outputs(block)
	persistent stop Wx Wy Wz;

	if block.Dwork(2).Data == 0
	  stop = uint8(0);
	  Wx = 0; Wy = 0; Wz = 0;
		block.Dwork(2).Data = 1;
	end

	udp = block.Dwork(1).Data;

	config_type  = 0;
	wind_type    = 1;

	if block.OutputPort(1).IsSampleHit || ...
		 block.OutputPort(2).IsSampleHit
		try,
			len = pnet(udp,'readpacket','noblock');
			if len > 0
				type = pnet(udp,'read',1,'uint8');

				switch type
					case {config_type}
						stop = pnet(udp,'read',1,'uint8');
					case {wind_type}
						Wx = pnet(udp,'read',1,'int32');
						Wy = pnet(udp,'read',1,'int32');
						Wz = pnet(udp,'read',1,'int32');
				end
			end
		end
		block.OutputPort(2).Data = stop;
		block.OutputPort(1).Data(1) = Wx;
		block.OutputPort(1).Data(2) = Wy;
		block.OutputPort(1).Data(3) = Wz;
	end

	if block.InputPort(1).IsSampleHit
		host = block.DialogPrm(1).Data;
		port = block.DialogPrm(3).Data;

		lat = 0;
		lon = 0;
		alt = 0;
		time = 0;
		hdg = 0;

		config_type  = 0;
		request_type = 1;

		lat  = block.InputPort(1).Data(1);
		lon  = block.InputPort(1).Data(2);
		alt  = block.InputPort(1).Data(3);
		time = block.InputPort(1).Data(4);
		hdg  = block.InputPort(1).Data(5);

		reqData = [uint8(request_type);
							 flipud(typecast(lat,'uint8')');
							 flipud(typecast(lon,'uint8')');
							 flipud(typecast(alt,'uint8')');
							 flipud(typecast(time,'uint8')');
							 flipud(typecast(hdg,'uint8')')
		];

		if udp~=-1,
			try,
				pnet(udp,'write',reqData);                     % Write to write buffer
				pnet(udp,'writepacket',host,port,'noblock');   % Send buffer as UDP packet
			end
		else
			fprintf(1,'Socket not connected\n');
		end
	end

function Terminate(block)
	udp = block.Dwork(1).Data;
	pnet(udp,'close');
